﻿using cutter;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace ur3
{
    class Program
    {

        public static float[] idlepos = new float[] { 0.090f, -0.417f, 0.339f, 1.24f, 2.83f, -0.07f };

        public static float[] idlepos2 = new float[] { -0.011f, -0.374f, 0.298f, 0.66f, 1.40f, 0.44f };

        public static float[] idlepos3 = new float[] { 0.270f, -0.195f, 0.295f, 1.20f, 2.90f, 0.00f };


        static void GetPath(ref List<float> p, char seg = ',', string pipename = "testpipe")
        {
            string s1=Pipe.ClientRead(pipename);

            while (true)
            {
                int segi = s1.IndexOf(seg);
                p.Add(Convert.ToSingle(s1.Substring(0, segi)));
                if (segi + 1 == s1.Length)
                    break;
                s1 = s1.Substring(segi + 1);
            }
        }


        static void SendPath(float []p, char seg = ',', string pipename = "testpipe")
        {
            string s1 = "";
            for (int i = 0; i < p.Length; i++)
                s1 = s1 + p[i] + seg;
            Pipe.ClientWrite(s1, pipename);
        }


        static void Move(string ip)
        {
            List<float> p = new List<float>();

            GetPath(ref p);

            UR3Control URController = new UR3Control(ip);

            URController.MovePath(ref p);

            URController.Close_client();
        }


        static void On(string ip)
        {
            UR3Control URController = new UR3Control(ip);

            URController.PowerOn();

            URController.Close_client();
        }

        static void Off(string ip)
        {
            UR3Control URController = new UR3Control(ip);

            URController.PowerOff();

            URController.Close_client();
        }


        static void RecordTCP(string ip)
        {
            UR3Control URController = new UR3Control(ip);            

            for(int i=0;i<10000;i++)
            {
                float[] tcp = URController.GetTCP();
                string str = tcp[0] + ", " + tcp[1] + ", " + tcp[2] + ", " + tcp[3] + ", " + tcp[4] + ", " + tcp[5];
                Console.Write(str);
                Console.Write("\r");
                Thread.Sleep(300);
            }

            URController.Close_client();
        }

        static void GetTCP(string ip, int n=1)
        {
            UR3Control URController = new UR3Control(ip);

            float[] tcp = URController.GetTCP(n);

            URController.Close_client();
            
            //UR3Control.ComparePos(tcp, new float[] { 0.30138f, -0.11524f, 0.34560f, 1.0764f, -3.0911f, 0.0596f}, 10, 0);

            SendPath(tcp);
        }


        static void Main(string[] args)
        {
            if (args.Length != 2)
                return;

            string ip = args[0];
            int choice = Convert.ToInt32(args[1]);

            switch(choice)
            {
                case 0:
                    RecordTCP(ip);
                    return;
                case 1:
                    Move(ip);
                    return;
                case 2:
                    On(ip);
                    return;
                case 3:
                    Off(ip);
                    return;
                case 4:
                    GetTCP(ip);
                    return;
                default:
                    RecordTCP(ip);
                    return;
            }
        }
    }
}
